#include "cheadbox.h"
#include <buola/io/cipendpoint.h>
#include <buola/zz/operators.h>
#include <buola/zz/zreal.h>
#include <buola/zz/zchar.h>

#pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"

namespace buola { namespace robot { namespace pacohead {

CHeadBox::CHeadBox()
    :   BBox("head")
    ,   oJoints(this,"joints out",7,std::bind(&CHeadBox::OnPullJoints,this))
    ,   mSetJoints(this,"set joints",std::bind(&CHeadBox::OnSetJoints,this,_1))
    ,   mStop(this,"stop",std::bind(&CHeadBox::OnStop,this))
    ,   mDoReset(this,"doreset",std::bind(&CHeadBox::OnDoReset,this))
    ,   oStatus(this,"status out",std::bind(&CHeadBox::OnPullStatus,this))
{
    InitHead();
}

void CHeadBox::InitHead()
{
    mSocket.open(io::CIPEndPoint("pacohead",5678));
    msg_info() << "head initialized!!!\n";
}

void CHeadBox::OnSetJoints(const std::array<double,7> &pJoints)
{
    mSocket << "movejoints";
    for(int i=0;i<7;i++)
        mSocket << " " << pJoints[i];
    mSocket << "\n";
}

void CHeadBox::OnStop()
{
    mSocket << "stop\n";
}

void CHeadBox::OnDoReset()
{
    mSocket << "doreset\n";
}

void CHeadBox::OnPullJoints()
{
    mSocket << "getjointpositions\n";
    std::string lString;
    getline(mSocket,lString);
    std::vector<double> lVector;
    auto lB=lString.begin(),lE=lString.end();
    parse(lB,lE,zz::ZReal<double>()%zz::ZChar<char>(' '),lVector);
    if(lVector.size()!=7) 
        throw XIO("received wrong response from head server");
    oJoints=lVector;
}

void CHeadBox::OnPullStatus()
{
    mSocket << "getstatus\n";
    std::string lString;
    getline(mSocket,lString);
    oStatus=to_<int>(lString);
}

/*namespace sdh*/ } /*namespace robot*/ } /*namespace buola*/ }
